import rclpy                                                                #导入ros2的python客户端库
from rclpy.node import Node                                                 #导入节点类
from geometry_msgs.msg import PoseStamped                                   #导入标准位置信息
from nav2_simple_commander.robot_navigator import BasicNavigator            #导入nav2下机器人导航的基层导航库
from tf_transformations import quaternion_from_euler,euler_from_quaternion                        #导入坐标变换工具从欧拉转换为四元数
import rclpy.time
from tf2_ros import Buffer,TransformListener
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
from datetime import datetime
from patrol_interfaces.srv import SpeechText
#创建一个类继承节点类

class PatrolNode(BasicNavigator):
    #初始化函数
    def __init__(self,name):
        super().__init__(name)
        #声明参数
        self.declare_parameter('init_point',[0.0, 0.0, 0.0])                            #初始点参数
        self.declare_parameter('target_points',[0.0, 0.0, 0.0, 1.0, 1.0, 1.57])         #目标点参数
        self.declare_parameter('img_path','')                                           #图像存放路径
        
        #获取参数
        self.init_point_=self.get_parameter('init_point').value
        self.target_points_=self.get_parameter('target_points').value
        self.img_path=self.get_parameter('img_path').value                                                                    
        self.sub_img_= self.create_subscription(Image,'/camera_sensor/image_raw',self.img_callback,1) #订阅图像信息
        self.last_image_=None
        self.buffer_=Buffer()                                                                          #创建buffer
        self.tf_listener_=TransformListener(self.buffer_,self)                                          #订阅TF
        self.speaker_client_=self.create_client(SpeechText,'speech_text')                               #创建一个客户端对象，设置接口类，订阅speech_text服务                                                                 #发布文字信息到合成语言的服务器（后面会在创建一个语音合成的节点）    

    
        #获取最新的图像信息
    def img_callback(self,image):
        self.last_image_=image                                          #获取最新的图像信息
    
        #保存图像
    def save_image(self):
        self.cv_bridge=CvBridge()                                        #实例化CvBridge
        cv_image=self.cv_bridge.imgmsg_to_cv2(self.last_image_)         #将图像格式进行转换
        formatted_time = datetime.now().strftime("%Y-%m-%d_%H-%M-%S")   #获取当前时间并且转换时间的格式为年月日十分秒
        pose=self.get_current_pose()                                    #获取当前位置信息
        cv2.imwrite(                                                    #保存图像并且拼接图像名字
            f'{self.img_path}_img{pose.translation.x:.2f}_{pose.translation.y:.2f}_{formatted_time}.png',cv_image
        )

        #获取当前的位置信息
    def get_current_pose(self):
        while rclpy.ok:
            try:
                result=self.buffer_.lookup_transform('map','base_footprint',
                                                      rclpy.time.Time(seconds=0.0),rclpy.time.Duration(seconds=1.0))    #获取当map和base_footprint之间的关系
                transform=result.transform                                                                              #获取TF
                self.get_logger().info(f'平移变换{transform.translation}')
                self.get_logger().info(f'旋转变换{transform.rotation}')
                rotation_euler=euler_from_quaternion([                                                                  #将四元数转换成欧拉
                    transform.rotation.x,
                    transform.rotation.y,
                    transform.rotation.z,
                    transform.rotation.w,]
                )
                self.get_logger().info(f'旋转RPY{rotation_euler}')
                return transform                                                                                        #返回TF
            except Exception as e:
                self.get_logger().info(f'获取旋转矩阵失败的原因{str(e)}')                                                   #打印错误信息

        
    def get_pose_by_xyyaw(self,x,y,yaw):
        #返回PoseStamped对象

        pose=PoseStamped()                                                             #定义标准位置
        pose.header.frame_id='map'                                                     #map坐标系下的位姿
        pose.pose.position.x=x
        pose.pose.position.y=y

        quat=quaternion_from_euler(0.0,0.0,yaw)                                        #将欧拉角转换为四元数
        #返回的顺序是xyzw
        pose.pose.orientation.x=quat[0]
        pose.pose.orientation.y=quat[1]
        pose.pose.orientation.z=quat[2]
        pose.pose.orientation.w=quat[3]
        
        return pose                                                                    #return  PoseStamped
    
    def init_pose(self):
        #初始化位姿
       self.init_point_=self.get_parameter('init_point').value                                              #获取参数
       init_pose=self.get_pose_by_xyyaw(self.init_point_[0],self.init_point_[1],self.init_point_[2])        #转化欧拉角为四元数
       self.setInitialPose(init_pose)                                                                        #调用函数初始化位姿
       self.waitUntilNav2Active()                                                                            #等待导航激活




    #获取目标点集合的函数
    def get_target_points(self):
        points=[]                                                                      #创建一个容器
        self.target_points_=self.get_parameter('target_points').value                  #获取目标点参数
        for index in range(int(len(self.target_points_)/3)):                           #将参数组合成三个一组的数组
            x=self.target_points_[index*3]
            y=self.target_points_[index*3+1]
            yaw=self.target_points_[index*3+2]
            points.append([x,y,yaw])                                                    #将点位放到一个容器中
            self.get_logger().info(f'获取到目标点{index}:{x},{y},{yaw}')
        return points



    #导航到目标点的函数
    def nav_to_pose(self,target_point):
        self.goToPose(target_point)                                                     #导航到目标点
        while not self.isTaskComplete():                                                #导航过程中
            feedback=self.getFeedback()
            self.get_logger().info(f'剩余导航距离：{feedback.distance_remaining}')         #持续反馈距离信息
        result=self.getResult()
        self.get_logger().info(f'导航结果：{result}')                                      #打印导航结果
    

    #将需要语音播报的内容发布给语音合成服务器的函数
    def speech_text(self,text):
        while not self.speaker_client_.wait_for_service(timeout_sec=1.0):               #如果服务器未上线那么将等待服务器上线
            self.get_logger().info('等待语音合成服务上线中...')
        request=SpeechText.Request()                                                      #创建请求信息
        request.text=text                                                               #传入请求
        future=self.speaker_client_.call_async(request)                                 #异步发送请求
        rclpy.spin_until_future_complete(self,future)                                   #等待服务完成
        if future.result() is not None:                                                 #判断result是否为空，为空则服务器响应失败
            response=future.result().result                                                    
            if response ==True:                                                       #判断是否成功并且打印结果信息
                self.get_logger().info(f'合成{text}成功')

            else:
                self.get_logger().info(f'合成{text}失败')
                
        else:
            self.get_logger().info('语音合成服务响应失败')

     

def main(args=None):
    rclpy.init(args=args)
    node=PatrolNode('patrol_node')                                                      #实例化节点类
    node.speech_text('正在初始化位置')                                                    #请求语音合成服务进行播报                                              
    node.init_pose()
    node.speech_text('初始化位置完成')                                                     #初始化位姿
    while rclpy.ok():
        points=node.get_target_points()                                                 #每一次循环获取目标点参数
        for point in points:
            x,y,yaw=point[0],point[1],point[2]                                         
            target_point=node.get_pose_by_xyyaw(x,y,yaw)                                 #转换目标点
            node.speech_text(f'正在前往{x},{y},{yaw}') 
            node.nav_to_pose(target_point)                                               #导航到目标点
            node.speech_text(f'到达{x},{y},{yaw}，正在采集图像') 
            node.save_image()                                                            #采集目标点的参数
            node.speech_text('图像采集完成') 
   
    rclpy.shutdown()
